#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "ROS_Bounding_Box.hpp"
#include "ros/callback_queue.h"
using namespace cv;
namespace enc=sensor_msgs::image_encodings;
BoundingBoxDetection::BoundingBoxDetection()
  :imageTrans(node),
   lower(4,200,120),
   upper(9,255,150)
{
  namedWindow("window1", 1);
  pub=imageTrans.advertise("boundingBox", 1);
  sub=imageTrans.subscribe("camera/image_raw", 1, &BoundingBoxDetection::CallBack, this);
  finished=true;
}
void BoundingBoxDetection::CallBack(const sensor_msgs::ImageConstPtr& msg)
{
  finished=false;
  cv_bridge::CvImagePtr cvptr;
  try
  {
        cvptr = cv_bridge::toCvCopy(msg, enc::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  //imshow("window1", mat2);

  //Mat mat2 
  cvptr->image= this->Processing(cvptr->image);

  //imshow("window1", mat2);
  //=mat2;

  //sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(mat2, "mono8");
  
  pub.publish(cvptr->toImageMsg());
  finished=true;
  //
}
BoundingBoxDetection::~BoundingBoxDetection()
{

}
void BoundingBoxDetection::messageLoop(){
   if(finished){
	ros::getGlobalCallbackQueue()->callAvailable();
	}
}
Mat BoundingBoxDetection::Processing(Mat mat1)
{
  Mat out(mat1);
  std::vector<std::vector<cv::Point> > contours;
  Mat hsv(out.size(),out.type());
  Mat binary(out.size(),CV_8UC1);
  Mat binary2(out.size(),CV_8UC1);
  cvtColor(out,hsv,CV_BGR2HSV);
  Mat hsv2(hsv.size(),hsv.type());
  bilateralFilter(hsv, hsv2, 5, 50, 50);
  inRange(hsv,lower,upper,binary);
  bilateralFilter(binary, binary2, 5, 50, 50);



  Mat contourOut(binary2.clone());
  findContours(contourOut, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
  std::vector<vector<Point> >::iterator contour=contours.begin();
  std::vector<Point> largest(0);
  int area=0;
  int max=area;


  for(;contour!=contours.end();++contour)
  {

	area=contourArea(*contour);
	if(area>max)
	{
		max=area;
		largest=*contour;

	}
  }
  //This (may) not be working
  if(largest.empty())
  {
	//drawContours(out,cl,0,Scalar(255,0,0),5);
	ROS_ERROR("DRAWING");
	Rect rect=boundingRect(largest);
	rectangle(out,rect.tl(),rect.br(),Scalar(255,0,0),1,8,0);
  }
  return out;
}
int main(int argc, char** argv)
{
  ros::init(argc,argv,"test");
  BoundingBoxDetection bb;
  while(ros::ok()){
	bb.messageLoop();
  }
  return 0;
}

